#include "RGBControl.h"

static uint8_t done;
static uint16_t step;
static uint8_t curRed, furRed, goRed;
static uint8_t curGreen, furGreen, goGreen;
static uint8_t curBlue, furBlue, goBlue;

Color *curColor,*futureColor,*goColor;

void initColor(void){

    curRed = extractRED(WHITE);
    furRed = extractRED(BLACK);

    curGreen = extractGREEN(WHITE);
    furGreen = extractGREEN(BLACK);

    curBlue = extractBLUE(WHITE);
    furBlue = extractBLUE(BLACK);

    step = 0x01;
    done = 0x01;
}

void setColor(Color *c, uint32_t newColor){
    c->red = extractRED(newColor);
    c->green = extractGREEN(newColor);
    c->blue = extractBLUE(newColor);
}

void updateColor(void){
	RED = goRed;
   	GREEN = goGreen;
    BLUE = goBlue;
}

void InterpolateColor(void){
    if(++step <= STEPS){
        InterpolateRed();
        InterpolateGreen();
        InterpolateBlue();
    }
    else{
        done = 0x02;
    }
}

void setFutureColor(uint32_t newColor){

    step = 0x01;
    done = 0x01;

    curRed = furRed;
    curGreen = furGreen;
    curBlue = furBlue;

    furRed = extractRED(newColor);
    furGreen = extractGREEN(newColor);
    furBlue = extractBLUE(newColor);
}

uint8_t checkDone(void){
    return done == 0x02;
}

//P0 + (((P1 - P0) / STEPS) * step)
void InterpolateRed(void) {
    static int32_t scaleCur=0,scaleFuture=0,result=0;

    scaleCur = curRed << SCALE_FACTOR;
    scaleFuture = furRed << SCALE_FACTOR;

    result = scaleCur + (((scaleFuture - scaleCur) / STEPS) * step);
    goRed = result >> SCALE_FACTOR;
}


void InterpolateGreen(void){
    static int32_t scaleCur=0,scaleFuture=0,result=0;

    scaleCur = curGreen * SCALE_FACTOR;
    scaleFuture = furGreen * SCALE_FACTOR;

    result = scaleCur + (((scaleFuture - scaleCur) / STEPS) * step);
    goGreen = (uint8_t) (result / SCALE_FACTOR);
}

void InterpolateBlue(void){
    static int32_t scaleCur=0,scaleFuture=0,result=0;

    scaleCur = curBlue * SCALE_FACTOR;
    scaleFuture = furBlue * SCALE_FACTOR;

    result = scaleCur + (((scaleFuture - scaleCur) / STEPS) * step);
    goBlue = (uint8_t)(result / SCALE_FACTOR);
}

unsigned char extractRED(unsigned long color){
    return color >> 16;
}

unsigned char extractGREEN(unsigned long color){
    return ((color & 0x00ff00) >> 8);
}

unsigned char extractBLUE(unsigned long color){
    return (color & 0x0000ff);
}
